Project 1: Pose Graph Optimization (scratch + g2o)

TEAM-ID:
TEAM-NAME:
YOUR-ID:
YOUR-NAME:

(Although you work in groups, both the students have to submit to Moodle, hence there's name field above)

Submission

Zip a folder of the following:

  1. Files that you were provided with: Project-1.ipynb, the folders misc and dataset. Rest of the files asked in the questions below must be generated when i run the code. If generation of any file is computationally intensive, add filename_backup.extension where filename.extension is the expected name of file when i run the code. (For example, next point.)
  2. Add opt_backup.g2o (described below) in outermost directory. Here, opt.g2o is the expected name of the file when I run the code.
  3. For images of any results (like plots), save it in ./misc folder.

On Moodle, all you have to submit is the jupyter notebook. But make sure to call the necessary functions explicitly (as specified in the notebook). The name of the zipped file being submitted to Moodle Assignment portal MUST BE ID_Teamname_Firstname. More details here.

On GitHub classrooms, the latest commit before the deadline will be considered as the submission.

The deadline is Oct 16, 23:55 IST. Please get started ASAP, there is no way you can finish this project during the last few days before the deadline.

General instructions

This ipython notebook (.ipynb) on GitHub is self-sufficient and has all the information you need to get started with the assignment, you don't need any corresponding PDF doc. Just fire up the notebook and get going!

General information like installation instructions in supplementary notebook "Project-1_Code-Walkthrough". Please take a look at it before you start this assignment.

Whenever I mention some func() below, I am referring to the "helper functions" in another supplementary notebook "Project-1_Code-Walkthrough" provided to you. Whenever I ask you to insert image below, it is better to save the image in misc and load it using ![file_name](file_location) instead of directly pasting.

[[CP-]] refers to CheckPoint, you have to ensure you do the tasks at each of the [[CP-]] places below. Not ensuring [[CP-B]] (CheckPoint-Basic) will incur heavy penalty and potentially 0 for that sub-section, and [[CP-M]] (CheckPoint-Marks) has a particular mark weightage depending on your results at that particular CP.

If you face any issues related to coding/installation, please raise an issue here. For any conceptual doubts, you can ask on Moodle or Teams as usual.

0. Introduction

In this project, we are going to use a non-linear weighted least squares optimization approach to solve the problem of getting a better estimate of our robot's trajectory. Least squares formulations are widely used for optimization, be it computer vision or robotics or machine learning. We will dive deep into it during this project and you will have complete clarity on optimization for vector-valued residual functions.

In this "Introduction" section, I am going to provide an introduction for SLAM problem for a robot operating in the 2D world. It is 2. section in this Project. The 1D SLAM problem (1.) is far much simpler to understand and will be described directly in the 1. section.

In a 2D world, a robot has 3 degrees of freedom, i.e. its pose in the world can be expressed by the state vector $\mathbf{x}=(x, y, \theta)^{\mathrm{T}}$. For the scope of this project, we are interested only in the robot's trajectory through the $2 \mathrm{D}$ world, and NOT in distinct landmarks or the surronding map of the environment, i.e. we are only interested in "L"ocalization part of SLAM.

Therefore, we can represent it as a graph where the vertices represent robot poses $\mathbf{x}_{i}$ and edges represent the spatial constraints between these poses. Such a map is generally called a pose graph.

Two different kinds of constraints are necessary for pose graph SLAM. The first are odometric constraints that connect two successive states $\mathbf{x}_{i}$ and $\mathbf{x}_{i+1}$ via a motion model. Furthermore, in order to perform loop closing, the robot has to recognize places it already visited before. This place recognition is also a part of the front-end and provides the second type of constraint, the loop closure constraints. These constraints connect two not necessarily successive poses $\mathbf{x}_{i}$ and $\mathbf{x}_{j}$.

SLAM-trajectory-lc.png SLAM-trajectory-robust.png (Source: Sunderhauf 2012)

You will start from the inaccurate pose graph with odometry and loop closure information and by the end of this Project, you end up with an optimized pose graph (see above images) which should look close to ground truth trajectory. You can watch this video to get an intuition for what we're about to do.

Okay, that's enough of theory. Let's get out hands dirty with the code!

In [2]:
import matplotlib.pyplot as plt
import math
import os
import jax.numpy as jnp #see supplementary notebook to see how to use this
import jax

import numpy as np

# If you're `importing numpy as np` for debugging purposes, 
# while submitting, please remove 'import numpy' and replace all np's with jnp's.(more in supplementary notebook)

plt.style.use('seaborn')

1. Pose Graph Optimization for 1D SLAM

A solved example for 1D SLAM which optimizes for pose variables using weighted least squares method (Gauss Newton) has been explained in the class. It has been made available here. Your first task is to code this from scratch. [[CP-M]]

For this section, you have to calculate Jacobian analytically yourself and use it. However, you can check how correct jax's jacobian. Its usage is explained in the supplementary notebook.

In [ ]:
##############################################################################
# TODO: Code for Section 1                                                   #

odometer = np.array([1.1, 1, 1.1, -2.7, 0])
pos = np.array([0, 1.1, 2.1, 3.2, 0.5])

weights = np.array([[100, 0, 0, 0, 0, 0],
                   [0, 100, 0, 0, 0, 0],
                   [0, 0, 100, 0, 0, 0],
                   [0, 0, 0, 100, 0, 0],
                   [0, 0, 0, 0, 100, 0],
                   [0, 0, 0, 0, 0, 1000],
                  ])

iterations = 5
for i in range(iterations):
    fx = np.array([ pos[0] + odometer[0], pos[1] + odometer[1], pos[2] + odometer[2], pos[3] + odometer[3], pos[0] + odometer[4], pos[0]])
    
    J = np.array([[1, -1, 0, 0, 0],
                   [0, 1, -1, 0, 0],
                   [0, 0, 1, -1, 0],
                   [0, 0, 0, 1, -1],
                   [1, 0, 0, 0, -1],
                   [1, 0, 0, 0, 0],
                  ])
    
    # H @ delta_x = -b
    H = J.T @ weights @ J
    fx_i = np.array([ fx[0] - pos[1], fx[1] - pos[2], fx[2] - pos[3], fx[3] - pos[4], fx[4] - pos[4], fx[5] - 0])
    
    b = J.T @ weights.T @ fx_i

    delta_x = -b @ np.linalg.inv(H)
    print('\n')
    print("Iteration", i)
    print('\n')
    print(delta_x)
    pos = pos + delta_x
    print(pos)

##############################################################################
#                             END OF YOUR CODE                               #
##############################################################################

Iteration 0


[ 6.9388939e-18 -1.0000000e-01 -2.0000000e-01 -3.0000000e-01
 -4.0000000e-01]
[6.9388939e-18 1.0000000e+00 1.9000000e+00 2.9000000e+00 1.0000000e-01]


Iteration 1


[-7.10542736e-18  1.27897692e-16  3.19744231e-17 -2.77111667e-16
 -8.88178420e-17]
[-1.66533454e-19  1.00000000e+00  1.90000000e+00  2.90000000e+00
  1.00000000e-01]


Iteration 2


[-3.08148791e-33 -4.97379915e-17 -9.94759830e-17  8.17124146e-17
 -3.90798505e-17]
[-1.66533454e-19  1.00000000e+00  1.90000000e+00  2.90000000e+00
  1.00000000e-01]


Iteration 3


[-4.62223187e-33 -4.97379915e-17 -9.94759830e-17  8.17124146e-17
 -3.55271368e-18]
[-1.66533454e-19  1.00000000e+00  1.90000000e+00  2.90000000e+00
  1.00000000e-01]


Iteration 4


[-4.62223187e-33 -4.97379915e-17 -9.94759830e-17  8.17124146e-17
 -3.55271368e-18]
[-1.66533454e-19  1.00000000e+00  1.90000000e+00  2.90000000e+00
  1.00000000e-01]

2. Pose Graph Optimization for 2D SLAM

Things are about to get interesting!

2.1 Coding from scratch

Objective

A robot is travelling in a oval trajectory. It is equipped with wheel odometry for odometry information and RGBD sensors for loop closure information. Due to noise in wheel odometry it generates a noisy estimate of the trajectory. Our task is to use loop closure pairs to correct the drift.

We pose this problem as a graph optimization problem. In our graph, poses are the vertices and constraints are the edges.

Given:

In practical scenarios, we'd obtain the following from our sensors after some post-processing:

  1. Initial position
  2. Odometry Contraints/Edges: This "edge" information basically tells us relative transformation between two nodes. These two nodes are consecutive in the case of Odometry but not in the case of Loop Closure (next point).
  3. Loop Closure Contraints/Edges Remember that while optimizing, you have another kind of "anchor" edge as you've seen in 1. solved example.

You have been given a text file named edges.txt which has all the above 3 and it follows G2O's format (as explained in class, link here ).

Details:

  1. Using the following motion model, you have to first generate the "initialization" for all the poses/vertices using the "Given" information. Just like in the 1D case. $$x_{k+1} = x_{k} + \Delta x_{(k,k+1)} \cos(\theta_k) - \Delta y_{(k,k+1)} \sin(\theta_k) \\ y_{k+1} = y_{k} + \Delta y_{(k,k+1)} \cos(\theta_k) + \Delta x_{(k,k+1)} \sin(\theta_k) \\ \theta_{k+1} = \theta_{k}+ \Delta \theta_{(k,k+1)} \tag{3}$$

Even the loop closure nodes are related by the above model, except that it need not necessarily be consecutive notes k and k+1.

Save this initial trajectory as edges-poses.g2o.

If you plot the initialized poses using odometry information, you need to get as the right plot [[CP-M]] below (this is the "noisy trajectory"): (Left one is the ground truth) robot-poses-MR-P1.png (Use draw() helper function or g2o_viewer or EVO)

  1. Now calculate the residual and the Jacobian and update your parameters using LM.

Use LM algorithm. Regarding Jacobian calculation, you can use jax's jacobian as part of your main code. However, you still have to separately calculate it analytically and verify if it matches with jax's jacobian using [[CP-M]] frobenius norm frobNorm()). Calculation and verification is compulsory, but it is your choice to use whichever as part of your optimization. Use whichever is faster.

  1. Regarding LM iterations, stopping criterion, information matrix values.

    1. [[CP-B]] As your iterations proceed, you have to print relevant information (iteration number and error value: $F = \frac{1}{2} \mathbf{f}^{\top} \mathbf{\Omega} \mathbf{f} $ (notion page link) at every step).

    2. [[CP-B]] You have to show the plots (ground truth, noisy & optimized: all 3 in a single plot) at every 10 steps or so.

    3. [[CP-M]] You could start with information values of 500 for odom edges, 700 for loop closure edges, 1000 for anchor edge (same for all dimensions). However, you have to heavily experiment with these values. (Given that you somehow know loop closure information is way more reliable than odometry.). At the end of your experimentation, your error $F = \frac{1}{2} \mathbf{f}^{\top} \mathbf{\Omega} \mathbf{f} $ should by < 40. Explain your experimentation in detail using tables/plots etc if necessary.

Do not worry if you're not getting a perfect trajectory. Our parametrization was oversimplified for the sake of this project. With that being said, it is possible to get the error down to < 40 and make it at least look like an oval shaped trajectory, even if it doesn't perfectly resemble the ground truth. However, using g2o (next section), you will be getting a close to ground truth trajectory.

In [14]:
##############################################################################
# TODO: Code for Section 2.1                                                 #
##############################################################################

# CONSTANTS
NUM_VARS = 3
WEIGHT_ODOMETRY = 200
WEIGHT_LOOP_CLOSURE = 500
WEIGHT_ANCHOR = 1000

LM_LAMBDA = 1
NUM_ITERS = 100
TOL = 1e-6
In [15]:
# UTILITY FUNCTIONS

def read_vertex(fileName):
    f = open(fileName, 'r')
    A = f.readlines()
    f.close()

    x_arr = []
    y_arr = []
    theta_arr = []

    for line in A:
        if "VERTEX_SE2" in line:
            (ver, ind, x, y, theta) = line.split()
            x_arr.append(float(x))
            y_arr.append(float(y))
            theta_arr.append(float(theta.rstrip('\n')))

    return jnp.array([x_arr, y_arr, theta_arr])

def read_edge(fileName):
    f = open(fileName, 'r')
    A = f.readlines()
    f.close()

    ind1_arr = []
    ind2_arr = []
    del_x = []
    del_y = []
    del_theta = []

    for line in A:
        if "EDGE_SE2" in line:
            (edge, ind1, ind2, dx, dy, dtheta, _, _, _, _, _, _) = line.split()
            ind1_arr.append(int(ind1))
            ind2_arr.append(int(ind2))
            del_x.append(float(dx))
            del_y.append(float(dy))
            del_theta.append(float(dtheta))

    return (jnp.array( ind1_arr), jnp.array(ind2_arr), jnp.array(del_x), jnp.array(del_y), jnp.array(del_theta))

# plot trajectory using matplotlib
def draw(X, Y, THETA):
    ax = plt.subplot(111)
    ax.plot(X, Y, 'ro')
    plt.plot(X, Y, 'c-')

    for i in range(len(THETA)):
        x2 = 0.25*math.cos(THETA[i]) + X[i]
        y2 = 0.25*math.sin(THETA[i]) + Y[i]
        plt.plot([X[i], x2], [Y[i], y2], 'g->')

    plt.show()
    
# motion model
def next_pose(x, y, theta, del_x, del_y, del_theta):
    x_new = x + del_x * jnp.cos(theta) - del_y * jnp.sin(theta)
    y_new = y + del_y * jnp.cos(theta) + del_x * jnp.sin(theta)
    theta_new = theta + del_theta
    return jnp.array([x_new, y_new, theta_new])

def write_poses(poses,file_name):
    with open(file_name, 'w') as f:
        for i in range(poses.shape[0]):
            st = "VERTEX_SE2 " + str(i) + " " + str(poses[i][0]) + " " + str(poses[i][1]) + " " + str(poses[i][2])
            f.write("%s\n" % st)
        
        # write edges and FIX
        f2 = open('edges.txt', 'r')
        A = f2.readlines()
        f2.close()
        
        for line in A:
            if "VERTEX_SE2" not in line:
                f.write("%s" % line)
In [16]:
# READ INPUT FILE AND GENERATE POSES

vertex = read_vertex('edges.txt')
edges = read_edge('edges.txt')
x = vertex[0][0]
y = vertex[1][0]
theta = vertex[2][0]

poses = []
poses.append(jnp.array([x, y, theta]))

for i in range(edges[0].shape[0]):
    ind1 = edges[0][i]
    ind2 = edges[1][i]
    
    if(abs(ind2 - ind1) > 1):
        break # do not include loop closure constrains
    
    del_x = edges[2][i]
    del_y = edges[3][i]
    
    del_theta = edges[4][i]
    
    x, y, theta = next_pose(x, y, theta, del_x, del_y, del_theta) # use odometry to get the next pose
    poses.append(jnp.array([x, y, theta]))
    
poses = jnp.array(poses)
anchor = poses[0, :]
print('Poses (Vertices):', poses.shape)
draw(poses[:,0], poses[:,1], poses[:,2])
write_poses(poses, 'edges-poses.g2o')

##############################################################################
#                             END OF YOUR CODE                               #
##############################################################################
Poses (Vertices): (120, 3)

edges-poses.png

In [17]:
# KEY FUNCTIONS

# Order of edges:
## x_anchor
## y_anchor
## theta_anchor
## for every edge:
### x_i
### y_i
### theta_i

# Order of pose variables
## for every edge:
### x_i
### y_i
### theta_i
# WRITE ALL VERTICES IN g2o FORMAT

def derivative_with_initial(x_i, y_i, theta_i, x_f, y_f, theta_f, del_x, del_y, del_theta):
    result = jnp.eye(NUM_VARS)
    
    result = jax.ops.index_update(result, jax.ops.index[0, 2], - del_x * jnp.sin(theta_i) - del_y * jnp.cos(theta_i))    
    result = jax.ops.index_update(result, jax.ops.index[1, 2], - del_y * jnp.sin(theta_i) + del_x * jnp.cos(theta_i))
    return result
    
def derivative_with_final(x_i, y_i, theta_i, x_f, y_f, theta_f, del_x, del_y, del_theta):
    result = -1 * jnp.eye(NUM_VARS)
    return result

def information_matrix(edges, weight_odom, weight_loop, weight_anchor):
    # edge x edge matrix
    diagonal = []
    
    # anchor edge
    diagonal.extend([weight_anchor] * NUM_VARS)
    
    for i in range(edges[0].shape[0]):
        ind1 = edges[0][i]
        ind2 = edges[1][i]
        if abs(ind2 - ind1) == 1:
            # odom edges
            diagonal.extend([weight_odom] * NUM_VARS)
        else:
            # loop closure edges
            diagonal.extend([weight_loop] * NUM_VARS)
    
    return jnp.diag(jnp.array(diagonal))

def residual(current_poses, edges, anchor):
    residue = []
    
    # anchor edges
    residue.extend(list(current_poses[0] - anchor))
    
    # other edges
    for i in range(edges[0].shape[0]):
        ind1 = edges[0][i]
        ind2 = edges[1][i]
        
        del_x = edges[2][i]
        del_y = edges[3][i]
        del_theta = edges[4][i]
        
        estimated_pose_2 = next_pose(*current_poses[ind1], del_x, del_y, del_theta)
        diff = list(estimated_pose_2 - current_poses[ind2])
        residue.extend(diff)
    
    return jnp.asarray(residue)


def jacobian(current_poses, edges):
    num_constraints = (edges[0].shape[0] * NUM_VARS) + NUM_VARS # for anchor edges
    num_variables = current_poses.shape[0] * NUM_VARS
    
    # constraints are ordered as in `edges`
    # variables are ordered as in `current_poses`
    
    J = jnp.zeros((num_constraints, num_variables))
    
    # for anchor edges
    J = jax.ops.index_update(J, jax.ops.index[:NUM_VARS, :NUM_VARS], np.eye(NUM_VARS))
    
    # for other edges
    for i in range(edges[0].shape[0]):
        ind1 = edges[0][i]
        ind2 = edges[1][i]
        del_x = edges[2][i]
        del_y = edges[3][i]
        del_theta = edges[4][i]
        
        J = jax.ops.index_update(
                J,
                jax.ops.index[NUM_VARS + i * NUM_VARS:NUM_VARS + i * NUM_VARS + NUM_VARS, ind1 * NUM_VARS: ind1 * NUM_VARS + NUM_VARS],
                derivative_with_initial(*poses[ind1], *poses[ind2], del_x, del_y, del_theta)
            )
        
        J = jax.ops.index_update(
                J,
                jax.ops.index[NUM_VARS + i * NUM_VARS:NUM_VARS + i * NUM_VARS + NUM_VARS, ind2 * NUM_VARS: ind2 * NUM_VARS + NUM_VARS],
                derivative_with_final(*poses[ind1], *poses[ind2], del_x, del_y, del_theta)
            )
    
    return J

def get_error(poses, edges, weights, anchor):
    f = residual(poses, edges, anchor)
    return 0.5 * f.T @ weights @ f

def step_lm(poses, edges, weights, anchor, lamda):
    f = residual(poses, edges, anchor)
    J = jacobian(poses, edges)
    
    step = - jnp.linalg.inv(J.T @ weights @ J + lamda * jnp.eye(J.shape[1])) @ J.T @ weights.T @ f
    return step.reshape((-1, NUM_VARS))

Comparing with Jax jacobian

In [18]:
get_jacobian = jax.jacfwd(residual, argnums=0)
J_theirs = get_jacobian(poses, edges, anchor)
J_theirs = J_theirs.reshape((420, -1, 1))[:, :, 0]

J_ours = jacobian(poses, edges)
In [19]:
def frobNorm(P1, P2, str1="mat1", str2="mat2"):
    jnp.set_printoptions(suppress=True)
    val = jnp.linalg.norm(P1 - P2, 'fro')
    print(f"Frobenius norm between {str1} and {str2} is: {val}")

print('Jacobian using jax auto differentiation ("theirs") has shape', J_theirs.shape)
print('Jacobian calculated analytically ("ours") has shape', J_ours.shape)
print(frobNorm(J_theirs, J_ours, 'theirs', 'ours'))

fig, ax = plt.subplots(1, 2)
ax[0].imshow(J_theirs)
ax[0].set_title('Theirs')
ax[1].imshow(J_ours)
ax[1].set_title('Ours')
plt.show()
Jacobian using jax auto differentiation ("theirs") has shape (420, 360)
Jacobian calculated analytically ("ours") has shape (420, 360)
Frobenius norm between theirs and ours is: 0.0
None
In [22]:
# plot trajectory using matplotlib
def draw_traj(X, Y, THETA):
    fig, ax = plt.subplots(nrows=1, ncols=3, figsize=(25, 6))
    
    gt_poses = read_vertex('gt.txt')
    gt_poses = gt_poses.T
    
    # Plot Ground truth
    ax[0].set_title("Ground Truth")
    ax[0].plot(gt_poses[:,0], gt_poses[:,1], 'ro')
    ax[0].plot(gt_poses[:,0], gt_poses[:,1], 'c-')
    
    for i in range(len(THETA)):
        x2 = 0.25*math.cos(gt_poses[i][3]) + gt_poses[i][0]
        y2 = 0.25*math.sin(gt_poses[i][3]) + gt_poses[i][1]
        ax[0].plot([gt_poses[i][0], x2], [gt_poses[i][1], y2], 'g->')
    
    # Plot Initial estimate
    ax[1].set_title("Initial Estimate")
    ax[1].plot(poses[:,0], poses[:,1], 'ro')
    ax[1].plot(poses[:,0], poses[:,1], 'c-')
    for i in range(len(THETA)):
        x2 = 0.25*math.cos(poses[i][3]) + poses[i][0]
        y2 = 0.25*math.sin(poses[i][3]) + poses[i][1]
        ax[1].plot([poses[i][0], x2], [poses[i][1], y2], 'g->')
    
    # Plot optimised trajectory
    ax[2].set_title("Optimised Trajectory")
    ax[2].plot(X, Y, 'ro')
    ax[2].plot(X, Y, 'c-')

    for i in range(len(THETA)):
        x2 = 0.25*math.cos(THETA[i]) + X[i]
        y2 = 0.25*math.sin(THETA[i]) + Y[i]
        ax[2].plot([X[i], x2], [Y[i], y2], 'g->')
    plt.show()
    
    
def optimise_lm(poses, edges, anchor, num_iters=NUM_ITERS, lamda=LM_LAMBDA, tol=TOL):
    weights = information_matrix(edges, WEIGHT_ODOMETRY, WEIGHT_LOOP_CLOSURE, WEIGHT_ANCHOR)
    
    poses_history = [poses]
    errors_history = [get_error(poses, edges, weights, anchor), ]

    print('Initial Guess')
    draw_traj(poses[:, 0], poses[:, 1], poses[:, 2])
    
    for _ in range(num_iters):
        new_poses = poses + step_lm(poses, edges, weights, anchor, lamda)
        new_error = get_error(new_poses, edges, weights, anchor)
        print('ITER', _, 'ERROR', new_error)
        draw_traj(new_poses[:, 0], new_poses[:, 1], new_poses[:, 2])

        if len(errors_history) > 0:
            if (new_error > errors_history[-1]):
                lamda = lamda * 2
            else:
                lamda = lamda / 3
                
        errors_history.append(new_error)
        poses_history.append(new_poses)
        
        if np.linalg.norm(new_poses - poses) < tol:
            break
        poses = new_poses
        
    return poses_history, errors_history
In [23]:
%%time
poses_history, errors_history = optimise_lm(poses, edges, anchor, num_iters=100)
plt.plot(errors_history)
plt.show()
best = jnp.argmin(jnp.asarray(errors_history))
draw(poses_history[best][:, 0], poses_history[best][:, 1], poses_history[best][:, 2])
write_poses(poses_history[best], 'opt.g2o')
Initial Guess
ITER 0 ERROR 57318.88
ITER 1 ERROR 3733.161
ITER 2 ERROR 2841.4297
ITER 3 ERROR 2726.1467
ITER 4 ERROR 1783.5677
ITER 5 ERROR 1173.5681
ITER 6 ERROR 1192.7623
ITER 7 ERROR 1542.2688
ITER 8 ERROR 1521.9971
ITER 9 ERROR 960.2212
ITER 10 ERROR 550.5163
ITER 11 ERROR 387.5808
ITER 12 ERROR 328.70746
ITER 13 ERROR 307.65588
ITER 14 ERROR 300.9121
ITER 15 ERROR 299.34927
ITER 16 ERROR 298.77707
ITER 17 ERROR 297.01132
ITER 18 ERROR 293.08713
ITER 19 ERROR 287.08347
ITER 20 ERROR 279.96365
ITER 21 ERROR 273.1108
ITER 22 ERROR 267.63397
ITER 23 ERROR 263.9286
ITER 24 ERROR 261.8648
ITER 25 ERROR 261.3108
ITER 26 ERROR 262.29037
ITER 27 ERROR 264.5443
ITER 28 ERROR 267.04147
ITER 29 ERROR 268.1975
ITER 30 ERROR 266.99274
ITER 31 ERROR 264.08588
ITER 32 ERROR 261.296
ITER 33 ERROR 259.33047
ITER 34 ERROR 256.95358
ITER 35 ERROR 253.95287
ITER 36 ERROR 253.92747
ITER 37 ERROR 261.88586
ITER 38 ERROR 278.4364
ITER 39 ERROR 296.03894
ITER 40 ERROR 302.01587
ITER 41 ERROR 292.08414
ITER 42 ERROR 285.62534
ITER 43 ERROR 306.0583
ITER 44 ERROR 328.7117
ITER 45 ERROR 309.12335
ITER 46 ERROR 269.3819
ITER 47 ERROR 274.73605
ITER 48 ERROR 369.73904
ITER 49 ERROR 562.7957
ITER 50 ERROR 804.83765
ITER 51 ERROR 917.0484
ITER 52 ERROR 680.74225
ITER 53 ERROR 606.4347
ITER 54 ERROR 1512.4176
ITER 55 ERROR 1802.615
ITER 56 ERROR 1017.13214
ITER 57 ERROR 500.54486
ITER 58 ERROR 503.26675
ITER 59 ERROR 1055.0892
ITER 60 ERROR 2521.3962
ITER 61 ERROR 5948.742
ITER 62 ERROR 11108.788
ITER 63 ERROR 8788.02
ITER 64 ERROR 3490.4053
ITER 65 ERROR 3373.2756
ITER 66 ERROR 3753.9353
ITER 67 ERROR 2867.1208
ITER 68 ERROR 1835.5338
ITER 69 ERROR 2943.8608
ITER 70 ERROR 2833.4597
ITER 71 ERROR 2082.391
ITER 72 ERROR 1251.7422
ITER 73 ERROR 669.0556
ITER 74 ERROR 410.4485
ITER 75 ERROR 338.06296
ITER 76 ERROR 324.26736
ITER 77 ERROR 322.29474
ITER 78 ERROR 322.1717
ITER 79 ERROR 322.25162
ITER 80 ERROR 322.30576
ITER 81 ERROR 322.33176
ITER 82 ERROR 322.34714
ITER 83 ERROR 322.3603
ITER 84 ERROR 322.37372
ITER 85 ERROR 322.38773
ITER 86 ERROR 322.4011
ITER 87 ERROR 322.4139
ITER 88 ERROR 322.4252
ITER 89 ERROR 322.43506
ITER 90 ERROR 322.44342
ITER 91 ERROR 322.45056
ITER 92 ERROR 322.45663
ITER 93 ERROR 322.46118
ITER 94 ERROR 322.46527
ITER 95 ERROR 322.46844
ITER 96 ERROR 322.47116
ITER 97 ERROR 322.47318
ITER 98 ERROR 322.4752
ITER 99 ERROR 322.47647
CPU times: user 22min 48s, sys: 4min 2s, total: 26min 50s
Wall time: 21min 46s

Analyze optimised trajectory with EVO

In [ ]:
! cp 'dataset/gt.txt' 'gt.g2o'
! python3 'misc/g2o_to_kitti.py' 'gt.g2o' 'gt.kitti'
! python3 'misc/g2o_to_kitti.py' 'opt.g2o' 'opt.kitti'
Figure(640x480)
saved 'gt.kitti' from 'gt.g2o'
Traceback (most recent call last):
  File "misc/g2o_to_kitti.py", line 74, in <module>
    (X, Y, THETA) = readG2o(argv[1])
  File "misc/g2o_to_kitti.py", line 27, in readG2o
    f = open(fileName, 'r')
FileNotFoundError: [Errno 2] No such file or directory: 'opt.g2o'
In [ ]:
! evo_rpe kitti gt.kitti opt.kitti
RPE w.r.t. translation part (m)
for delta = 1 (frames) using consecutive pairs
(not aligned)

       max	0.288269
      mean	0.120154
    median	0.119680
       min	0.017908
      rmse	0.135883
       sse	2.197228
       std	0.063460

In [ ]:
! evo_ape kitti gt.kitti opt.kitti
APE w.r.t. translation part (m)
(not aligned)

       max	11.930598
      mean	9.816228
    median	10.302679
       min	5.481529
      rmse	9.966596
       sse	11919.963177
       std	1.724732

In [ ]:
! evo_traj kitti gt.kitti opt.kitti -v --plot --plot_mode xy
--------------------------------------------------------------------------------
Loaded 120 poses from: gt.kitti
Loaded 120 poses from: opt.kitti
--------------------------------------------------------------------------------
name:	gt
infos:
	nr. of poses	120
	path length (m)	52.976517997999295
	pos_end (m)	[-2.8 -4.5  0. ]
	pos_start (m)	[-8.  5.  0.]
--------------------------------------------------------------------------------
name:	opt
infos:
	nr. of poses	120
	path length (m)	54.0369052140993
	pos_end (m)	[ 6.7866635 -5.863554   0.       ]
	pos_start (m)	[-5.418309e-08 -7.896482e-09  0.000000e+00]
In [ ]:
! g2o_viewer opt.g2o
zsh:1: command not found: g2o_viewer

2.1 Answer

Give a detailed answer addressing the above questions. When I run the above code, it should follow points described above (such as plots at every 10 steps) and (When I run the above code, it should) write the optimized poses to a file named opt.g2o. As a backup, save another file opt_backup.g2o in an offline manner beforehand.

That apart, save important plots and add them here so that it can supplement your answer, for example you could add plots at crucial stages of optimization. You have to add useful metrics/plots from EVO (refer to supplementary notebook). Using EVO, the bare minimum you have to report is mean absolute pose error (ape) and mean relative pose error (rpe). However, you are encouraged to use tools like evo_traj and more and add more plots/metrics. Marks will be awarded based on overall analysis & presentation which would reflect your understanding.

Note that EVO and g2o_viewer (below) could help you in debugging.

Add answer for 2.1 here:

2.2 Using g2o's optimization: g2o binary or g2o viewer

Installation setup is described in supplementary notebook. More details for 2.2.1 and 2.2.2 can be found in the supplementary notebook.

2.2.1 Optimizing edges.txt

First task is to optimize the poses of dataset you've been working with so far.

2.2.2 Optimizing intel and sphere datasets

You have been given two datasets in the data folder. You have to use g2o_viewer to optimize these both. You have to experiment with the options/parameters available in the GUI. More instructions in supplementary notebook. You have to experiment till you get the trajectories which look like the following:

Drawing Drawing

2.2 Answer

Add images: take screenshot of the GUI of g2o_viewer after optimization for all 3 [[CP-M]] and add here. Briefly describe what you had to do (detailed answer is not expected). g2o could potentially give you close to ground truth trajectory for all 3, but if you are unable to get to close to ground truth, add the best you can get.

Intel

Comparison of trajectory made up of given poses (leftmost), after intial guess (middle) and after optimising (rightmost)

For optimisation we used Gauss Newton optimizer and achieved best results with just 5-6 iterations.

Original image of intel Initial guess on intel Optimised trajectory of intel
In [ ]:
## Sphere
Comparison of trajectory made up of given poses (leftmost), after intial guess (middle) and after optimising (rightmost)

# <??>

<table><tr>
<td> <img src="./misc/sphere_original.png" alt="Original image of sphere" style="width: 300px;"/> </td>
<td> <img src="./misc/sphere_initialguess.png" alt="Initial guess on sphere" style="width: 300px;"/> </td>
<td> <img src="./misc/sphere_optimised.png" alt="Optimised trajectory of sphere" style="width: 300px;"/> </td>
</tr></table>
  File "<ipython-input-2-08041e0a0398>", line 2
    Comparison of trajectory made up of given poses (leftmost), after intial guess (middle) and after optimising (rightmost)
                ^
SyntaxError: invalid syntax

* Important Information regarding Questions 3 & 4

Note that it is mandatory to attempt EITHER 3 OR 4, only one of it. If you attempt both, the question which you score more will be considered and the other as bonus question.

It is encouraged for those into robotics/deep learning research to prefer 4 over 3.

[Bonus*] 3. Deriving Motion model geometrically

* -> read information above under section "Important Information regarding Questions 3 & 4"

The current robot state is as follows: ($i$ and $k$ are interchangably used below, sorry I am too lazy to edit now 😛)
robot-situation.png

Can you derive the below equation using geometry? (Read on)

$$x_{k+1} = x_{k} + \Delta x_{(k,k+1)} \cos(\theta_k) - \Delta y_{(k,k+1)} \sin(\theta_k) \\ y_{k+1} = y_{k} + \Delta y_{(k,k+1)} \cos(\theta_k) + \Delta x_{(k,k+1)} \sin(\theta_k) \\ \theta_{k+1} = \theta_{k}+ \Delta \theta_{(k,k+1)} \tag{3}$$

In other words, we want to find $\delta$'s in terms of $\Delta$'s $$\delta x = \Delta x \cos(\theta) - \Delta y \sin(\theta) \\ \delta y = \Delta y \cos(\theta) + \Delta x \sin(\theta) \tag{2}$$

where $\delta$'s are the updates in our motion model equation: $$ x_{k+1} = x_{k} + \delta x \\ y_{k + 1} = y_k + \delta y \\ \theta_{k+1} = \theta_{k} + \delta \theta \tag{1}$$

Oh yes, $\theta$ is straightforward, i.e. $\delta \theta = \Delta \theta$ but why?

Using geometry, you could just draw and insert a self-explanatory image as the answer to this question.

If you can derive it without using geometry purely using transform matrices/algebra, that is fine too. Whatever you're comfortable.

3. Answer

Your answer here.

[Bonus*] 4. Research Paper Reading

* -> read information above under section "Important Information regarding Questions 3 & 4"

(Do not get intimidated, you are not expected to do a thorough research analysis for this task. A high level understanding is sufficient.)

"Past, Present & Future of SLAM: Towards the Robust Perception Age" is an exciting survey paper of 2016 which sums up, well, the "past, present & future" of SLAM. Your task is as follows:

  1. Go through the sections "IV. LONG-TERM AUTONOMY II: SCALABILITY" & "III. LONG-TERM AUTONOMY I: ROBUSTNESS". Don't worry, you are not expected to have a deep understanding. Skip the parts which you don't understand at all. Go through it at a high level, and take a slightly closer look at "Open Problems" in these sections.

  2. Read up common applications of deep learning for computer vision/robotics through blogs online (for example, first 4 points of this. Again, you are only expected to understand it at a high level, for example, 'semantic segmentation is an application of deep learning for computer vision which is the task of assigning a category to each of the pixels in the image'.

Firstly, summarize your understanding of the above two points.

Now, from the understanding you've gathered so far, how would you approach solving those "Open Problems"? Can these algorithms help in dealing with some of the issues you might have faced during this project? Can the deep learing based high level understanding of the world help in SLAM? In the context of long term autonomy, imagine tomorrow's world with a buddy robot R2-D2 which follows you wherever you go... Now imagine how easily the trajectory can diverge, how big the map could soon become and how the computation could easily become intractable.

Answer the above questions in the context of this project and those 2 sections of the survey paper.

4. Answer

Your answer here.

Fun section

Check the end of your Project-1 homepage on Notion. :)